#include <gtest/gtest.h>
#include <thread>
#include <pthread.h>
#include "rtbus/rtbus.h"
#include "rtbus/ethercat/ecat.h"
#include "jkutil/timespec.h"

/**
 * 需要真机进行测试
 */
class ECatTest : public ::testing::Test
{
protected:
    void SetUp() override { cfg.adapter_name = "enp88s0"; }

    void TearDown() override {}
    rtbus::ECat ecat;
    rtbus::RtCommuItf::BusCfg cfg;
};

TEST(ECatStartTest, DISABLE_open)
{
    rtbus::ECat ecat;
    rtbus::RtCommuItf::BusCfg cfg;
    cfg.dev_id = 88;
    cfg.channel_id = 0;
    ASSERT_EQ(ecat.open(cfg), 0);
}

/**
 * @brief 测试ecat连接6个伺服的active函数与OP模式下的8ms通信，时间同步
 */
TEST_F(ECatTest, DISABLE_active)
{
    ASSERT_EQ(ecat.open(cfg), 0);
    for (int i = 0; i < 4; i++)
    {
        rtbus::RtCommuItf::SlaveInfo slave;
        slave.cycletime_us = 8000;
        slave.shifttime_us = slave.cycletime_us * 0.25;
        slave.slaveid = i;
        slave.vendor_id = 0x661100;
        slave.product_code = 0x10003202;
        slave.revision_number = 0x00000001;
        slave.enable_dc = true;
        ecat.config_slave(slave);
        ecat.config_txpdo(i << 24 | 0x604000, 2);
        ecat.config_txpdo(i << 24 | 0x607A00, 4);
        ecat.config_txpdo(i << 24 | 0x60B200, 2);
        ecat.config_txpdo(i << 24 | 0x606000, 1);
        ecat.config_txpdo(i << 24 | 0x700000, 1);

        ecat.config_rxpdo(i << 24 | 0x604100, 2);
        ecat.config_rxpdo(i << 24 | 0x606400, 4);
        ecat.config_rxpdo(i << 24 | 0x607700, 2);
        ecat.config_rxpdo(i << 24 | 0x606100, 1);
        ecat.config_rxpdo(i << 24 | 0x700100, 1);
    }
    ASSERT_EQ(ecat.active(), 0);
    struct timespec next;
    clock_gettime(CLOCK_MONOTONIC, &next);
    int cyccnt = 30 * 1000 * 1000 / 8000;
    uint16_t cword = 0x06;
    uint16_t sword = 0, sword2 = 0;
    uint8_t opmode = 8;
    sched_param sched;
    sched.sched_priority = 98;
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &sched);

    for (int i = 0; /*i < cyccnt + 1*/; i++)
    {
        int64_t toff = 0;
        ecat.recv(&toff);
        ecat.send();
        uint16_t act_sword = 0;
        ecat.get_rxpdo_data(0x604100, (uint8_t*)&act_sword);
        if (act_sword != sword)
        {
            printf("act_sword: 0x%x\n", act_sword);
            sword = act_sword;
        }
        uint16_t act_sword2 = 0;
        ecat.get_rxpdo_data(0x1604100, (uint8_t*)&act_sword2);
        if (act_sword2 != sword2)
        {
            printf("act_sword2: 0x%x\n", act_sword2);
            sword2 = act_sword2;
        }
        int32_t actpos = 0;
        ecat.get_rxpdo_data(0x606400, (uint8_t*)&actpos);

        ecat.set_txpdo_data(0x604000, (uint8_t*)&cword);
        ecat.set_txpdo_data(0x607A00, (uint8_t*)&actpos);
        ecat.set_txpdo_data(0x606000, (uint8_t*)&opmode);

        struct timespec dt;
        dt.tv_sec = 0;
        dt.tv_nsec = 8000 * 1000 + toff;
        next = timespec_add(next, dt);
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
    }
    //TODO
    printf("ecat test done\n");
}

TEST_F(ECatTest, getsdo)
{
    ecat.open(cfg);

    for (int i = 0; i < 2; i++)
    {
        rtbus::RtCommuItf::SlaveInfo slave;
        slave.cycletime_us = 1000;
        slave.shifttime_us = slave.cycletime_us * 0.25;
        slave.slaveid = i;
        slave.vendor_id = 0x661100;
        slave.product_code = 0x10003202;
        slave.revision_number = 0x00000001;
        slave.enable_dc = true;
        ecat.config_slave(slave);
    }
    bool stopflag = false;
    ASSERT_EQ(ecat.active(), 0);
    auto t = std::thread([this, &stopflag]() {
        sched_param sched;
        sched.sched_priority = 98;
        pthread_setschedparam(pthread_self(), SCHED_FIFO, &sched);
        timespec next;
        clock_gettime(CLOCK_MONOTONIC, &next);
        while (!stopflag)
        {
            int64_t toff = 0;
            ecat.recv(&toff);
            ecat.send();
            struct timespec dt;
            dt.tv_sec = 0;
            dt.tv_nsec = 1000 * 1000 + toff;
            next = timespec_add(next, dt);
            clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
        }
    });
    uint8_t opdis = 0;
    ASSERT_EQ(ecat.get_sdo(
                  1,
                  0x160000,
                  [&opdis](uint8_t* data, int size) {
                      printf("data: %d\n", data[4]);
                      opdis = data[4];
                      return 0;
                  },
                  1000,
                  3),
              0);
    while (!opdis) { usleep(1000); }
    ASSERT_NE(opdis, 0);
    stopflag = true;
    t.join();
}

TEST_F(ECatTest, setsdo)
{
    ecat.open(cfg);

    for (int i = 0; i < 2; i++)
    {
        rtbus::RtCommuItf::SlaveInfo slave;
        slave.cycletime_us = 1000;
        slave.shifttime_us = slave.cycletime_us * 0.25;
        slave.slaveid = i;
        slave.vendor_id = 0x661100;
        slave.product_code = 0x10003202;
        slave.revision_number = 0x00000001;
        slave.enable_dc = true;
        ecat.config_slave(slave);
    }
    bool stopflag = false;
    ASSERT_EQ(ecat.active(), 0);
    auto t = std::thread([this, &stopflag]() {
        sched_param sched;
        sched.sched_priority = 98;
        pthread_setschedparam(pthread_self(), SCHED_FIFO, &sched);
        timespec next;
        clock_gettime(CLOCK_MONOTONIC, &next);
        while (!stopflag)
        {
            int64_t toff = 0;
            ecat.recv(&toff);
            ecat.send();
            struct timespec dt;
            dt.tv_sec = 0;
            dt.tv_nsec = 1000 * 1000 + toff;
            next = timespec_add(next, dt);
            clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
        }
    });
    uint8_t opmode = 8;
    ASSERT_EQ(ecat.set_sdo(
                  1,
                  0x606000,
                  &opmode,
                  1,
                  [](uint8_t* data, int size) {
                      printf("data: %d\n", data[4]);
                      return 0;
                  },
                  1000,
                  3),
              0);
    while (!opmode) { usleep(1000); }
    ASSERT_NE(opmode, 0);
    stopflag = true;
    t.join();
}
class ECatMutiTest : public ::testing::Test
{
protected:
    void SetUp() override
    {
        cfg1.adapter_name = "enp88s0";
        cfg2.adapter_name = "enx00e04c880907";
    }

    void TearDown() override
    {
        ecat.close();
        ecat2.close();
    }
    rtbus::ECat ecat;
    rtbus::ECat ecat2;
    rtbus::RtCommuItf::BusCfg cfg1, cfg2;
};

TEST_F(ECatMutiTest, DISABLE_open)
{
    EXPECT_EQ(ecat.open(cfg1), 0);
    EXPECT_EQ(ecat2.open(cfg2), 0);
}
/**
 * @brief 测试多ecat主站连接，一路2个伺服，另一路4个伺服，的active函数与OP模式下的8ms通信，时间同步
 */
TEST_F(ECatMutiTest, DISABLE_active)
{
    ASSERT_EQ(ecat.open(cfg1), 0);
    ASSERT_EQ(ecat2.open(cfg2), 0);
    for (int i = 0; i < 4; i++)
    {
        rtbus::RtCommuItf::SlaveInfo slave;
        slave.cycletime_us = 1000;
        slave.shifttime_us = slave.cycletime_us * 0.25;
        slave.slaveid = i;
        slave.vendor_id = 0x661100;
        slave.product_code = 0x10003202;
        slave.revision_number = 0x00000001;
        slave.enable_dc = true;
        ecat.config_slave(slave);
        ecat.config_txpdo(i << 24 | 0x604000, 2);
        ecat.config_txpdo(i << 24 | 0x607A00, 4);
        ecat.config_txpdo(i << 24 | 0x60B200, 2);
        ecat.config_txpdo(i << 24 | 0x606000, 1);
        ecat.config_txpdo(i << 24 | 0x700000, 1);

        ecat.config_rxpdo(i << 24 | 0x604100, 2);
        ecat.config_rxpdo(i << 24 | 0x606400, 4);
        ecat.config_rxpdo(i << 24 | 0x607700, 2);
        ecat.config_rxpdo(i << 24 | 0x606100, 1);
        ecat.config_rxpdo(i << 24 | 0x700100, 1);
    }
    for (int i = 0; i < 2; i++)
    {
        rtbus::RtCommuItf::SlaveInfo slave;
        slave.cycletime_us = 1000;
        slave.shifttime_us = slave.cycletime_us * 0.25;
        slave.slaveid = i;
        slave.vendor_id = 0x661100;
        slave.product_code = 0x10003202;
        slave.revision_number = 0x00000001;
        slave.enable_dc = true;
        ecat2.config_slave(slave);
        ecat2.config_txpdo(i << 24 | 0x604000, 2);
        ecat2.config_txpdo(i << 24 | 0x607A00, 4);
        ecat2.config_txpdo(i << 24 | 0x60B200, 2);
        ecat2.config_txpdo(i << 24 | 0x606000, 1);
        ecat2.config_txpdo(i << 24 | 0x700000, 1);

        ecat2.config_rxpdo(i << 24 | 0x604100, 2);
        ecat2.config_rxpdo(i << 24 | 0x606400, 4);
        ecat2.config_rxpdo(i << 24 | 0x607700, 2);
        ecat2.config_rxpdo(i << 24 | 0x606100, 1);
        ecat2.config_rxpdo(i << 24 | 0x700100, 1);
    }

    ASSERT_EQ(ecat.active(), 0);
    ASSERT_EQ(ecat2.active(), 0);

    struct timespec next;
    clock_gettime(CLOCK_MONOTONIC, &next);
    int cyccnt = 130 * 1000 * 1000 / 1000;
    uint16_t cword = 0x06;
    uint16_t sword = 0, sword2 = 0;
    uint8_t opmode = 8;
    sched_param sched;
    sched.sched_priority = 98;
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &sched);
    for (int i = 0; /*i < cyccnt + 1*/; i++)
    {
        int64_t toff = 0;
        ecat.recv(&toff);
        ecat.send();
        uint16_t act_sword = 0;
        ecat.get_rxpdo_data(0x604100, (uint8_t*)&act_sword);
        if (act_sword != sword)
        {
            printf("act_sword: 0x%x\n", act_sword);
            sword = act_sword;
        }
        uint16_t act_sword2 = 0;
        ecat.get_rxpdo_data(0x1604100, (uint8_t*)&act_sword2);
        if (act_sword2 != sword2)
        {
            printf("act_sword2: 0x%x\n", act_sword2);
            sword2 = act_sword2;
        }
        int32_t actpos = 0;
        ecat.get_rxpdo_data(0x606400, (uint8_t*)&actpos);

        ecat.set_txpdo_data(0x604000, (uint8_t*)&cword);
        ecat.set_txpdo_data(0x607A00, (uint8_t*)&actpos);
        ecat.set_txpdo_data(0x606000, (uint8_t*)&opmode);

        int64_t toff2 = 0;
        ecat2.recv(&toff2);
        ecat2.sync_time(&ecat);
        ecat2.send();

        struct timespec dt;
        dt.tv_sec = 0;
        // if (i % 2 == 0)
        {
            dt.tv_nsec = 1000 * 1000 + toff;
        }
        // else
        // {
        // dt.tv_nsec = 1000 * 1000 + toff2;
        // }
        next = timespec_add(next, dt);
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
    }
}
